﻿// File: Time.cs
// Project: ROS_C-Sharp
// 
// ROS.NET
// Eric McCann <emccann@cs.uml.edu>
// UMass Lowell Robotics Laboratory
// 
// Reimplementation of the ROS (ros.org) ros_cpp client in C#.
// 
// Created: 10/14/2015
// Updated: 02/10/2016

using System;
using System.Threading;
using Messages.rosgraph_msgs; 


namespace RosSharp
{
    public class SimTime
    {
        public delegate void SimTimeDelegate(TimeSpan ts);

        private static object _instanceLock = new object();
        private static SimTime _instance;

        private bool checkedSimTime;
        private NodeHandle nh;
        private bool simTime;
        private Subscriber<Clock> simTimeSubscriber;

        public SimTime()
        {
            new Thread(() =>
            {
                while (!ROS.IsStarted() && !ROS.ShuttingDown)
                {
                    Thread.Sleep(100);
                }
                nh = new NodeHandle();
                if (!ROS.ShuttingDown)
                {
                    simTimeSubscriber = nh.Subscribe<Clock>("/clock", 1, SimTimeCallback);
                }
            }).Start();
        }

        public bool IsTimeSimulated
        {
            get { return simTime; }
        }

        public static SimTime Instance
        {
            get
            {
                lock (_instanceLock)
                {
                    if (_instance == null)
                    {
                        _instance = new SimTime();
                    }
                    return _instance;
                }
            }
        }

        public event SimTimeDelegate SimTimeEvent;

        private void SimTimeCallback(Clock time)
        {
            if (!checkedSimTime)
            {
                if (Param.Get("/use_sim_time", ref simTime))
                {
                    checkedSimTime = true;
                }
            }
            if (simTime && SimTimeEvent != null)
                SimTimeEvent.Invoke(TimeSpan.FromMilliseconds(time.clock.data.sec * 1000.0 + (time.clock.data.nsec / 100000000.0)));
        }
    }
}